import numpy as np

import cv2

#### W/H: 8192/5460
# cameraMatrix =np.array([
# [8529.19657994319, 0, 4096+9.89224588620221],
# [0, 8529.19657994319, 2730+7.35020245943671],
# [0, 0, 1]], dtype=np.float32)
# distCoeffs = np.array([-0.0471627783238073,-0.0154369488516742, 0.000200148071496319, 0.000117138032146703,-0.0454659387045057],dtype=np.float32)


#### W/H: 1024/682
sc = 1024.0 / 8192.0
cameraMatrix =np.array([
[8529.19657994319 * sc, 0, 4105.89224588620221*sc],
[0, 8529.19657994319*sc, 2737.35020245943671*sc],
[0, 0, 1]], dtype=np.float32)
distCoeffs = np.array([-0.0471627783238073,-0.0154369488516742, 0.000200148071496319, 0.000117138032146703,-0.0454659387045057],dtype=np.float32)


def get_pose(obj_points,img_points):
    ret,rvec,tvec,inners = cv2.solvePnPRansac(obj_points,img_points,cameraMatrix,distCoeffs,flags=cv2.SOLVEPNP_EPNP,reprojectionError=20)
    # ret,rvec,tvec = cv2.solvePnP(obj_points,img_points,cameraMatrix,distCoeffs,flags=cv2.SOLVEPNP_EPNP)

    R = cv2.Rodrigues(rvec)
    r = R[0].copy()
    campos = np.array([[0,0,0]]).T
    campos = np.dot(r.T,campos-tvec)
    return campos[:,0]

